# -- coding: utf-8 --
"""
# MIAHX© 控制面板消息的 FastDDS 发布/订阅器

**模块：MIAHX© / 框架基础服务层（FFST） / 交互框架信息表示模型（IFIRM）**

Version: 0.4.10,
Last updated: October 20, 2024

## Contributor(s)
* YAO Naiming <[naiming@iscas.ac.cn](mailto:naiming@iscas.ac.cn)>

## Change log
### v0.4.10
- .20: [优化] Reader 构造方法传入 topic_name
- .19: [更新] 新增 HudStartupMsgReader/Writer 和 HutStartupMsgReader/Writer
- .2: [更新] 适配新基类
### v0.4.9
-.26: [新增] 初始实现

---
Copyright © 2024-2025 MIAHX Contributors, ISCAS, and ASIC. All Rights Reserved.
Licensed under Apache-2.0.
"""

import os

import miahx_fastdds as dds
from loguru import logger

from ..participant import Participant
from ..reader import Reader, ReaderListener
from ..topics import Topics
from ..writer import Writer

_log = logger.bind(module_name=os.path.basename(__file__))


# **** START BEAM MP SERVER ========================================

class BeamMPServerStartupMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_START_BMP_SRV
        topic_data_type = dds.BeamMPServerStartupMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class BeamMPServerStartupMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_START_BMP_SRV, dds.BeamMPServerStartupMsgPubSubType())

    def write(self, name: str, map_path: str, auth_key: str, description='', port=30814, non_public=True,
              tags='Freeroam', max_cars=16, max_players=8, log_chat=True, enable_debug=False):
        data = dds.BeamMPServerStartupMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        data.map_path(map_path)
        data.description(description)
        data.port(port)
        data.non_public(non_public)
        data.auth_key(auth_key)
        data.tags(tags)
        data.max_cars(max_cars)
        data.max_players(max_players)
        data.log_chat(log_chat)
        data.enable_debug(enable_debug)
        # send
        self._writer.write(data)


# **** START BEAM NG ========================================

class BeamNGStartupMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_START_BNG
        topic_data_type = dds.BeamNGStartupMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class BeamNGStartupMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_START_BNG, dds.BeamNGStartupMsgPubSubType())

    def write(self, name: str,
              env_tod: str, env_fog_density: float, env_rain_nums: float, env_rain_water_plane_height: float,
              traffic_density: int, traffic_speed_limit: float, traffic_mode: str, traffic_aggression: float,
              traffic_spawn_freq: float, traffic_gap: float, profiles: list[dict]):
        data = dds.BeamNGStartupMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        #
        data.env_tod(env_tod)
        data.env_fog_density(env_fog_density)
        data.env_rain_nums(env_rain_nums)
        data.env_rain_water_plane_height(env_rain_water_plane_height)
        #
        data.traffic_density(traffic_density)
        data.traffic_speed_limit(traffic_speed_limit)
        data.traffic_mode(traffic_mode)
        data.traffic_aggression(traffic_aggression)
        data.traffic_spawn_freq(traffic_spawn_freq)
        data.traffic_gap(traffic_gap)
        #
        vehicles = []
        for item in profiles:
            vp = dds.BeamNGVehicle()
            vp.vid(item['vid'])
            vp.model(item['model'])
            vp.licence(item['licence'])
            vp.color(item['color'])
            vp.spawn_pos(item['spawn_pos'])
            vp.spawn_rot(item['spawn_rot'])
            vehicles.append(vp)
        data.vehicles(vehicles)

        # send
        self._writer.write(data)


# **** START HUD ========================================

class HudStartupMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_START_HUD
        topic_data_type = dds.HudStartupMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class HudStartupMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_START_HUD, dds.HudStartupMsgPubSubType())

    def write(self, name: str = 'ar-hud'):
        data = dds.HudStartupMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        # send
        self._writer.write(data)


# **** START HUT ========================================

class HutStartupMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_START_HUT
        topic_data_type = dds.HutStartupMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class HutStartupMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_START_HUT, dds.HutStartupMsgPubSubType())

    def write(self, name: str):
        data = dds.HutStartupMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        # send
        self._writer.write(data)


# **** START GNSS ========================================

class GNSSyncStartupMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_START_GNSS
        topic_data_type = dds.GNSSyncStartupMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class GNSSyncStartupMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_START_GNSS, dds.GNSSyncStartupMsgPubSubType())

    def write(self, periods: float, use_gnss=False):
        data = dds.GNSSyncStartupMsg()
        data.header(self._create_msg_header())
        # body
        data.periods(periods)
        data.use_gnss(use_gnss)
        # send
        self._writer.write(data)


# **** START DATA COLLECTOR ========================================


class DataCollectorStartupMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_START_COLLECTOR
        topic_data_type = dds.DataCollectorStartupMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class DataCollectorStartupMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_START_COLLECTOR, dds.DataCollectorStartupMsgPubSubType())

    def write(self, name: str, group: str, target_vid: str,
              record_id: str, record_tag: str, profiles: list[dict]):
        data = dds.DataCollectorStartupMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        data.group(group)
        data.target_vid(target_vid)
        data.record_id(record_id)
        data.record_tag(record_tag)
        sensors = []
        for item in profiles:
            if 'profile' not in item or 'profile_name' not in item:
                _log.warning(f'`profile` or `profile_name` not found')
                continue

            profile_name = item["profile_name"].lower()
            creator = f'_create_{profile_name}_profile'

            if not hasattr(self, creator):
                _log.warning(f'`{profile_name}` not supported')
                continue

            content = getattr(self, creator)(item['profile'])
            csp = dds.ConcreteSensorProfile()
            getattr(csp, profile_name)(content)

            sp = dds.SensorProfile()
            sp.name(item['name'])
            sp.sensor_type(item['sensor_type'])
            sp.channel_type(item['channel_type'])
            sp.channel_id(item['channel_id'])
            sp.modality(item['modality'])
            sp.data_format(item['data_format'])
            sp.fps(item['fps'])
            sp.allow_publishing(item['allow_publishing'])
            sp.allow_persistence(item['allow_persistence'])
            sp.profile(csp)
            sp.profile_name(profile_name)
            sensors.append(sp)
        data.sensors(sensors)
        # send
        self._writer.write(data)

    def _create_pvcc_basler_profile(self, profile):
        return self._create_basler_profile(profile)

    def _create_pvic_basler_profile(self, profile):
        return self._create_basler_profile(profile)

    @staticmethod
    def _create_basler_profile(profile):
        sp = dds.BaslerSensorProfile()
        sp.mode(profile.get('mode', 'listen'))
        sp.dev_id(profile.get('dev_id', 0))
        sp.resolution(profile.get('resolution', (640, 480)))
        sp.preview_quality(profile.get('preview_quality', 95))
        return sp

    def _create_pvgc_smart_eye_profile(self, profile):
        sp = dds.SmartEyeSensorProfile()
        sp.mode(profile.get('mode', 'listen'))
        sp.dev_name(profile.get('dev_name', 'SmartEye-120'))
        sp.dev_host(profile.get('dev_host', '192.168.124.12'))
        sp.dev_port(profile.get('dev_port', 8080))
        sp.srate(profile.get('srate', 60))
        sp.time_buffer(profile.get('time_buffer', 6))
        return sp

    def _create_pvgc_smart_eye_snapshot_profile(self, profile):
        sp = dds.SnapshotSensorProfile()
        sp.mode(profile.get('mode', 'poll'))
        sp.screen_size(profile.get('screen_size', (2560, 1600)))
        sp.snapshot_size(profile.get('snapshot_size', (835, 515)))
        sp.snapshot_desired_size(profile.get('snapshot_desired_size', (165, 110)))
        sp.positions(profile.get('positions', [3, 125, 855, 135, 1705, 135]))
        sp.n_channels(profile.get('n_channels', 3))
        sp.time_buffer(profile.get('time_buffer', 6))
        return sp

    def _create_pamc_dji_profile(self, profile):
        sp = dds.MicrophoneSensorProfile()
        sp.mode(profile.get('mode', 'poll'))
        sp.n_channels(profile.get('n_channels', 2))
        sp.sample_width(profile.get('sample_width', 2))
        sp.frame_rate(profile.get('frame_rate', 16000))
        sp.time_buffer(profile.get('time_buffer', 6))
        return sp

    def _create_peeg_neuracle_profile(self, profile):
        sp = dds.NeuracleSensorProfile()
        sp.mode(profile.get('mode', 'listen'))
        sp.dev_name(profile.get('dev_name', 'DSI-24'))
        sp.dev_host(profile.get('dev_host', '127.0.0.1'))
        sp.dev_port(profile.get('dev_port', 8844))
        sp.srate(profile.get('srate', 256))
        sp.n_channels(profile.get('n_channels', 25))
        sp.time_buffer(profile.get('time_buffer', 3))
        return sp

    def _create_pwbd_psych_tech_gsr_profile(self, profile):
        sp = dds.PsychTechWristSensorProfile()
        sp.mode(profile.get('mode', 'subscribe'))
        sp.dev_host(profile.get('dev_host', '127.0.0.1'))
        sp.dev_port(profile.get('dev_port', 1883))
        sp.points(profile.get('points', 100))
        sp.n_channels(profile.get('n_channels', 4))  # diff
        return sp

    def _create_pwbd_psych_tech_ppg_profile(self, profile):
        return self._create_psych_tech_wrist_profile(profile)

    @staticmethod
    def _create_psych_tech_wrist_profile(profile):
        sp = dds.PsychTechWristSensorProfile()
        sp.mode(profile.get('mode', 'subscribe'))
        sp.dev_host(profile.get('dev_name', 'PSYCHTECH_WRIST_PPG'))
        sp.dev_host(profile.get('dev_host', '127.0.0.1'))
        sp.dev_port(profile.get('dev_port', 1883))
        sp.points(profile.get('points', 100))
        sp.n_channels(profile.get('n_channels', 100))
        return sp

    def _create_pphs_physiolab_profile(self, profile):
        sp = dds.PhysiolabSensorProfile()
        sp.mode(profile.get('mode', 'listen'))
        sp.dev_host(profile.get('dev_host', '127.0.0.1'))
        sp.dev_port(profile.get('dev_port', 5555))
        sp.points(profile.get('points', 600))
        sp.channels(profile.get('channels', ['EMG0', 'EMG1']))
        return sp

    def _create_vbng_camera_profile(self, profile):
        sp = dds.BNGCameraSensorProfile()
        sp.general(self._create_vbng_general(profile))
        sp.resolution(profile.get('resolution', (800, 600)))
        sp.quality(profile.get('quality', 95))
        sp.use_shm(profile.get('use_shm', True))
        sp.fov_y(profile.get('fov_y', 60))
        sp.nfp(profile.get('nfp', (0.1, 100)))
        sp.allow_depth(profile.get('allow_depth', False))
        sp.allow_annotation(profile.get('allow_annotation', True))
        sp.allow_instance(profile.get('allow_instance', False))
        return sp

    def _create_vbng_lidar_profile(self, profile):
        sp = dds.BNGLidarSensorProfile()
        sp.general(self._create_vbng_general(profile))
        sp.use_shm(profile.get('use_shm', True))
        sp.is_annotated(profile.get('is_annotated', True))
        sp.rays_per_second(profile.get('rays_per_second', 220000))
        sp.frequency(profile.get('frequency', 20))
        sp.max_distance(profile.get('max_distance', 120))
        return sp

    def _create_vbng_radar_profile(self, profile):
        sp = dds.BNGRadarSensorProfile()
        sp.general(self._create_vbng_general(profile))
        sp.use_shm(profile.get('use_shm', True))
        sp.resolution(profile.get('resolution', (200, 200)))
        sp.fov_y(profile.get('fov_y', 70))
        sp.nfp(profile.get('nfp', (0.1, 150.0)))
        return sp

    def _create_vbng_classical_profile(self, profile):
        sp = dds.BNGClassicalSensorProfile()
        sp.general(self._create_vbng_general(profile))
        sp.allow_timer(profile.get('allow_timer', False))
        sp.allow_ecu(profile.get('allow_ecu', False))
        sp.allow_imu(profile.get('allow_imu', False))
        sp.allow_damage(profile.get('allow_damage', False))
        sp.allow_gforces(profile.get('allow_gforces', False))
        return sp

    def _create_vbng_gps_profile(self, profile):
        sp = dds.BNGGPSSensorProfile()
        sp.general(self._create_vbng_general(profile))
        sp.offsets(profile.get('offsets', (0, 0)))
        return sp

    def _create_vbng_imu_adv_profile(self, profile):
        sp = dds.BNGIMUAdvSensorProfile()
        sp.general(self._create_vbng_general(profile))
        return sp

    def _create_vbng_navigation_profile(self, profile):
        sp = dds.BNGNavigationSensorProfile()
        sp.general(self._create_vbng_general(profile))
        sp.ai_speed_limit(profile.get('ai_speed_limit', 50.0))
        return sp

    @staticmethod
    def _create_vbng_general(profile):
        bsgp = dds.BNGSensorGeneralProfile()
        if 'general' in profile:
            bsgp.visible(profile['general'].get('visible', False))
            bsgp.pos(profile['general'].get('pos', (0, 0, 0)))
            bsgp.dir(profile['general'].get('dir', (0, 0, 0)))
        else:
            bsgp.visible(False)
            bsgp.pos((0, 0, 0))
            bsgp.dir((0, 0, 0))
        return bsgp


# **** START PROC ACK ========================================

class ProcessStartupAckMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_STARTUP_ACK
        topic_data_type = dds.ProcessStartupAckMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class ProcessStartupAckMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_STARTUP_ACK, dds.ProcessStartupAckMsgPubSubType())

    def write(self, name: str, pid: int):
        data = dds.ProcessStartupAckMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        data.pid(pid)
        # send
        self._writer.write(data)


# **** SHUTDOWN ========================================

class ProcessShutdownMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_SHUTDOWN
        topic_data_type = dds.ProcessShutdownMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class ProcessShutdownMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_SHUTDOWN, dds.ProcessShutdownMsgPubSubType())

    def write(self, name: str, pid: int):
        data = dds.ProcessShutdownMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        data.pid(pid)
        # send
        self._writer.write(data)


# **** SHUTDOWN ACK ========================================

class ProcessShutdownAckMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_SHUTDOWN_ACK
        topic_data_type = dds.ProcessShutdownAckMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class ProcessShutdownAckMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_SHUTDOWN_ACK, dds.ProcessShutdownAckMsgPubSubType())

    def write(self, name: str, pid: int):
        data = dds.ProcessShutdownAckMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        data.pid(pid)
        # send
        self._writer.write(data)


# **** HEARTBEAT ========================================

class ProcessHeartbeatMsgReader(Reader):
    def __init__(self, participant: Participant, on_data_listeners=None):
        topic_name = Topics.CPL_PROC_HEARTBEAT
        topic_data_type = dds.ProcessHeartbeatMsgPubSubType()
        super().__init__(participant, topic_name, topic_data_type,
                         listener=ReaderListener(topic_name, topic_data_type, on_data_listeners))


class ProcessHeartbeatMsgWriter(Writer):
    def __init__(self, participant: Participant):
        super().__init__(participant, Topics.CPL_PROC_HEARTBEAT, dds.ProcessHeartbeatMsgPubSubType())

    def write(self, name: str, pid: int, is_running=True):
        data = dds.ProcessHeartbeatMsg()
        data.header(self._create_msg_header())
        # body
        data.name(name)
        data.pid(pid)
        data.is_running(is_running)
        # send
        self._writer.write(data)
